#include "MyIMU.h"
#include <stdlib.h>

float Mval[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int32_t H = 0;
//kalman_union KM_Y, KM_P, KM_R;

void MyIMU_Init()
{
	float val[9];

	// deactivate internal pull-ups for twi as per note from atmega8 manual pg167
	cbi(PORTC, 4);
	cbi(PORTC, 5);

	lastUpdate = 0;
	now = 0;

	q0 = 1.0; q1 = 0.0; q2 = 0.0; q3 = 0.0;
	exInt = 0.0; eyInt = 0.0; ezInt = 0.0;

	#ifdef ADXL345
	ADXL345_Init(ADXL345_ADDRESS_ALT_LOW);
	#endif

	#ifdef BMA180
	BMA180_Init(BMA180_ADDRESS_SDO_LOW);
	#endif

	#if defined(ITG3200) || defined(MPU3050)
	ITG3200_Init(ITG3200_ADDR_AD0_HIGH);
	#endif

	#ifdef MPU6050
	MPU6050_Init(MPU6050_DEFAULT_ADDRESS);
	#endif

	/*	#if defined(HMC5843) || defined(HMC5883)
	HMC5883_Init(HMC5883L_ADDR, 0);
	#endif
	*/
	#ifdef BMP085
	BMP085_Init(BMP085_ADDR);
	#endif

	#if defined(MS5607) || defined(MS5611)
	MS56XX_Init(MS56XX_ADDR);
	#endif

	for (unsigned char i = 0; i < SMOOTH_SCALE; i++)
	{
		#ifdef ADXL345
		ADXL345_getValues(&val[0], &val[1], &val[2]);
		#endif

		#ifdef BMA180
		BMA180_getValues(&val[0], &val[1], &val[2]);
		#endif

		#if defined(ITG3200) || defined(MPU3050)
		ITG3200_getValues(&val[3], &val[4], &val[5]);
		#endif

		#ifdef MPU6050
		//		MPU6050_getMotion9(&val[0], &val[1], &val[2], &val[3], &val[4], &val[5], &val[6], &val[7], &val[8]);
		#endif
		/*
		#if defined(HMC5843) || defined(HMC5883)
		HMC5883_getValues(&val[6], &val[7], &val[8]);
		#endif
		*/
		for (unsigned char j = 0; j < 9; j++)
		Mval[j] = (Mval[j] * i + val[j]) / (i + 1);
	}
	//	KM_Y = kalman_init(0.1, 0.0125, 20.0, 0.0);
	//	KM_P = kalman_init(0.1, 0.0125, 20.0, 0.0);
	//	KM_R = kalman_init(0.1, 0.0125, 20.0, 0.0);

}

void MyIMU_AHRSupdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
	float norm;
	float hx, hy, hz, bx, bz;
	float vx, vy, vz, wx, wy, wz;
	float ex, ey, ez;
	//	float exAcc, eyAcc, ezAcc;
	//	float exMag, eyMag, ezMag;

	// auxiliary variables to reduce number of repeated operations
	float q0q0 = q0 * q0;
	float q0q1 = q0 * q1;
	float q0q2 = q0 * q2;
	float q0q3 = q0 * q3;
	float q1q1 = q1 * q1;
	float q1q2 = q1 * q2;
	float q1q3 = q1 * q3;
	float q2q2 = q2 * q2;
	float q2q3 = q2 * q3;
	float q3q3 = q3 * q3;

	now = millis();
	halfT = (now - lastUpdate) / 2000.0;	// chuyen sang sec
	lastUpdate = now;
	//	deltaT = 2 * halfT;						// dung cho tich phan PID

	// normalize the measurements
	norm = sqrt(ax * ax + ay * ay + az * az);
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;

	norm = sqrt(mx * mx + my * my + mz * mz);
	mx = mx / norm;
	my = my / norm;
	mz = mz / norm;

	// compute reference direction of flux
	hx = 2 * (mx * (0.5 - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
	hy = 2 * (mx * (q1q2 + q0q3) + my * (0.5 - q1q1 - q3q3) + mz * (q2q3 - q0q1));
	hz = 2 * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5 - q1q1 - q2q2));
	bx = sqrt((hx * hx) + (hy * hy));
	bz = hz;

	// estimated direction of gravity and flux (v and w)
	vx = 2 * (q1q3 - q0q2);
	vy = 2 * (q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;
	wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
	wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
	wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);

	// error is sum of cross product between reference direction of fields and direction measured by sensors
	ex = (ay * vz - az * vy) + (my * wz - mz * wy);
	ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
	ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
	//	exAcc = ay * vz - az * vy;
	//	eyAcc = az * vx - ax * vz;
	//	ezAcc = ax * vy - ay * vx;
	
	//	exMag = my * wz - mz * wy;
	//	eyMag = mz * wx - mx * wz;
	//	ezMag = mx * wy - my * wx;

	// integral error scaled integral gain
	exInt += ex * Ki;
	eyInt += ey * Ki;
	ezInt += ez * Ki;
	//	exInt += exAcc * kiAcc + exMag * kiMag;
	//	eyInt += eyAcc * kiAcc + eyMag * kiMag;
	//	ezInt += ezAcc * kiAcc + ezMag * kiMag;

	// adjusted gyroscope measurements
	gx += ex * Kp + exInt;
	gy += ey * Kp + eyInt;
	gz += ez * Kp + ezInt;
	//	gx = gx + exAcc * kpAcc + exMag * kpMag + exInt;
	//	gy = gy + eyAcc * kpAcc + eyMag * kpMag + eyInt;
	//	gz = gz + ezAcc * kpAcc + ezMag * kpMag + ezInt;

	// integrate quaternion rate and normalize
	q0 += (-q1 * gx - q2 * gy - q3 * gz) * halfT;
	q1 += (q0 * gx + q2 * gz - q3 * gy) * halfT;
	q2 += (q0 * gy - q1 * gz + q3 * gx) * halfT;
	q3 += (q0 * gz + q1 * gy - q2 * gx) * halfT;

	// normalize quaternion
	norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;
}

/*void MyIMU_getValues(float * values)
{
BMA180_getValues(&values[0], &values[1], &values[2]);
ITG3200_getValues(&values[3], &values[4], &values[5]);
HMC58X3_getValues(&values[6], &values[7], &values[8]);
}*/

float Smooth(unsigned char i, float val)
{
	Mval[i] = (Mval[i] * SMOOTH_SCALE + val) / (SMOOTH_SCALE + 1);
	return Mval[i];
}

int32_t SmoothH(int32_t val)
{
	H = (H * SMOOTH_SCALE_H + val) / (SMOOTH_SCALE_H + 1);
	return H;
}

float MyIMU_GetHeading()
{
	#if defined(HMC5843) || defined(HMC5883)
	float val[3];
	HMC5883_getValues(&val[0], &val[1], &val[2]);
	_delay_ms(60);
	float heading = atan2(val[1], val[0]);
	heading = heading * _180_DIV_PI_;
	return heading;
	#else
	return 0.0f;
	#endif
}

void MyIMU_getQuaternion(float * q)
{
	//	float val[9];
	//	MyIMU_getValues(val);
	#ifdef ADXL345
	ADXL345_getValues(&val[0], &val[1], &val[2]);
	#endif

	#ifdef BMA180
	BMA180_getValues(&val[0], &val[1], &val[2]);
	#endif

	#if defined(ITG3200) || defined(MPU3050)
	ITG3200_getValues(&val[3], &val[4], &val[5]);
	#endif

	#ifdef MPU6050
	MPU6050_getMotion9(&val[0], &val[1], &val[2], &val[3], &val[4], &val[5], &val[6], &val[7], &val[8]);
	#endif

	//	for (unsigned char i = 3; i < 6; i++)
	//		val[i] = Smooth(i, val[i]);
	//	Omega[0] = val[5];// * _180_DIV_PI_;
	//	Omega[1] = val[4];// * _180_DIV_PI_;
	//	Omega[2] = -val[3];// * _180_DIV_PI_;
	//	Omega[0] = kalman_update(&KM_Y, -val[3] * _180_DIV_PI_);	// dao lai dau truc X
	//	Omega[1] = kalman_update(&KM_P, val[4] * _180_DIV_PI_);
	//	Omega[2] = kalman_update(&KM_R, val[5] * _180_DIV_PI_);

	/*	#if defined(HMC5843) || defined(HMC5883)
	HMC5883_getValues(&val[6], &val[7], &val[8]);
	#endif
	*/
	//	for (unsigned char i = 0; i < 3; i++)
	//		val[i] = Smooth(i, val[i]);
	if (setup_status == 0)
	MyIMU_AHRSupdate(val[0], val[1], val[2], val[3], val[4], val[5], val[6], val[7], val[8]);
	else
	MyIMU_AHRSupdate(val[0], val[1], val[2], 0, 0, 0, val[6], val[7], val[8]);
	q[0] = q0;
	q[1] = q1;
	q[2] = q2;
	q[3] = q3;
}

void MyIMU_getYawPitchRoll(float * ypr)		// tinh ra degree
{
	//	float q[4];			// quaternion
	float gx, gy, gz;	// estimated gravity direction
	MyIMU_getQuaternion(q);

	gx = 2 * (q[1] * q[3] - q[0] * q[2]);
	gy = 2 * (q[0] * q[1] + q[2] * q[3]);
	gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

	ypr[0] = atan2(2 * (q[1] * q[2] - q[0] * q[3]), 2 * (q[0] * q[0] + q[1] * q[1]) - 1) * _180_DIV_PI_;	// YAW
	ypr[1] = atan(gx / sqrt(gy * gy + gz * gz)) * _180_DIV_PI_;											// PITCH
	ypr[2] = atan(gy / sqrt(gx * gx + gz * gz)) * _180_DIV_PI_;											// ROLL
}

/*void MyIMU_getEuler(float * angles)
{
//	float q[4];			// quaternion
MyIMU_getQuaternion(q);
angles[0] = atan2(2 * (q[1] * q[2] - q[0] * q[3]), 2 * (q[0] * q[0] + q[1] * q[1]) - 1) * _180_DIV_PI_ - HC_Yaw;	// psi
angles[1] = -asin(2 * (q[1] * q[3] + q[0] * q[2])) * _180_DIV_PI_ - HC_Pitch;										// theta
angles[2] = atan2(2 * (q[2] * q[3] - q[0] * q[1]), 2 * (q[0] * q[0] + q[3] * q[3]) - 1) * _180_DIV_PI_ - HC_Roll;	// phi
}*/


//========================================================================================================================//
//=====================================================5DOF===============================================================//
//========================================================================================================================//

void MyIMU_Init5DOF()
{
	firstSample = true;
}

void MyIMU_Init3DOF()
{
	firstSample = true;
}

void MyIMU_get5DOF() {
	MPU6050_getMotion9(&RwAcc[0], &RwAcc[1], &RwAcc[2], &Gyro_ds[0], &Gyro_ds[1], &Gyro_ds[2], &val[6], &val[7], &val[8]);
	getInclination5DOF();
}

void MyIMU_get3DOF() {
	MPU6050_getMotion9(&RwAcc[0], &RwAcc[1], &RwAcc[2], &Gyro_ds[0], &Gyro_ds[1], &Gyro_ds[2], &val[6], &val[7], &val[8]);
	for(int w=0;w<=2;w++) {
		RwEst[w] = RwAcc[w];
	}
	normalize3DVec(RwEst);
}

void getInclination5DOF() {
	int w = 0;
	float tmpf = 0.0;
	int signRzGyro;
	
	now = millis();
	deltaT = (now - lastUpdate) / 1000.0;	// chuyen sang sec
	lastUpdate = now;
	
	if (firstSample) { // the NaN check is used to wait for good data from the Arduino
		for(w=0;w<=2;w++) {
			RwEst[w] = RwAcc[w];    //initialize with accelerometer readings
		}
	}
	else{
		//evaluate RwGyro vector
		if(abs(RwEst[2]) < 0.1) {
			//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
			//in this case skip the gyro data and just use previous estimate
			for(w=0;w<=2;w++) {
				RwGyro[w] = RwEst[w];
			}
		}
		else {
			//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
			for(w=0;w<=1;w++){
				tmpf = Gyro_ds[w];                      //get current gyro rate in deg/s
				tmpf *= deltaT;							//get angle change in deg
				Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI;   //get angle and convert to degrees
				Awz[w] += tmpf;             //get updated angle according to gyro movement
			}
			
			//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
			//RzGyro is pozitive if  Axz in range -90 ..90 => cos(Awz) >= 0
			signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
			
			//reverse calculation of RwGyro from Awz angles, for formulas deductions see  http://starlino.com/imu_guide.html
			for(w=0;w<=1;w++){
				RwGyro[0] = sin(Awz[0] * PI / 180);
				RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
				RwGyro[1] = sin(Awz[1] * PI / 180);
				RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
			}
			RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
		}
		
		//combine Accelerometer and gyro readings
		for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

		normalize3DVec(RwEst);
	}
	
	firstSample = false;
}

void normalize3DVec(float * vector) {
	float R;
	R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
	vector[0] /= R;
	vector[1] /= R;
	vector[2] /= R;
}


float squared(float x){
	return x*x;
}